#include <unistd.h>
#include <sys/time.h>
#include <sys/statfs.h>
#include <termio.h>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/char.hpp"
#include "base_interfaces/msg/chassis.hpp"
#include "base_interfaces/msg/gimbal_pose.hpp"
#include "base_interfaces/msg/shooter.hpp"
using std::placeholders::_1;
using namespace std::chrono_literals;

void monitorKeyboard(char *key)
{
    while (rclcpp::ok())
    {
        termios new_settings;
        termios stored_settings;
        tcgetattr(0, &stored_settings);
        new_settings = stored_settings;
        new_settings.c_lflag &= (~ICANON);
        new_settings.c_cc[VTIME] = 0;
        tcgetattr(0, &stored_settings);
        new_settings.c_cc[VMIN] = 1;
        tcsetattr(0, TCSANOW, &new_settings);
        *key = getchar();
        tcsetattr(0, TCSANOW, &stored_settings);
    }
}


class ControlTest : public rclcpp::Node
{   

public:

    // 发包管理
    bool chassis_enable = false;
    bool gimbal_enbale = false;
    bool shooter_enable = false;

    // 底盘
    float chassis_x_speed = 0.0;
    float chassis_y_speed = 0.0;
    bool top = false;

    float recv_chassis_x_speed = 0.0;
    float recv_chassis_y_speed = 0.0;
    bool recv_top = false;

    // 云台
    float gimbal_pitch_angle = 0.0;
    float gimbal_yaw_angle = 0.0;

    float recv_gimbal_pitch_angle = 0.0;
    float recv_gimbal_yaw_angle = 0.0;

    bool use_angle_control = true;

    // 射击
    int bulletnum = 0;

    // 权限
    char permissions = 0;

    ControlTest(std::string name) : Node(name)
    {
        std::thread keyThread(monitorKeyboard, &c);
        keyThread.detach();

        topAndSpeed = this->create_publisher<base_interfaces::msg::Chassis>("TopAndSpeed",1);
        setGimbalAngle = this->create_publisher<base_interfaces::msg::GimbalPose>("SetGimbalAngle",1);
        setGimbal_yawSpeed_pitchAngle = this->create_publisher<base_interfaces::msg::GimbalPose>("SetGimbal_YawSpeed_PitchAngle", 1);
        shooter = this->create_publisher<base_interfaces::msg::Shooter>("ShootSome",1);
        publishKey = this->create_publisher<std_msgs::msg::Char>("my_key",1);
        
        gimbalPoseSub = this->create_subscription<base_interfaces::msg::GimbalPose>("GetGimbalAngle", 10, std::bind(&ControlTest::getGimbalPoseCallback, this, _1));
        gimbal_pitchAngle_yawSpeed_Sub = this->create_subscription<base_interfaces::msg::GimbalPose>("GetGimbal_YawSpeed_PitchAngle", 10, std::bind(&ControlTest::getGimbalPoseCallback2, this, _1));
        chassisSub = this->create_subscription<base_interfaces::msg::Chassis>("getSpeed", 10, std::bind(&ControlTest::getChassisSpeedCallback, this, _1));

        init();

        chassisTimer = this->create_wall_timer(50ms,std::bind(&ControlTest::topAndSpeedCallback,this));
        printTimer = this->create_wall_timer(100ms,std::bind(&ControlTest::print,this));
    }
private:
    char c = 'f';
    rclcpp::TimerBase::SharedPtr chassisTimer;
    rclcpp::TimerBase::SharedPtr printTimer;
    rclcpp::Publisher<base_interfaces::msg::Chassis>::SharedPtr topAndSpeed;
    rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr setGimbalAngle;
    rclcpp::Publisher<base_interfaces::msg::Shooter>::SharedPtr shooter;
    rclcpp::Publisher<base_interfaces::msg::GimbalPose>::SharedPtr setGimbal_yawSpeed_pitchAngle;
    rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr gimbalPoseSub;
    rclcpp::Subscription<base_interfaces::msg::Chassis>::SharedPtr chassisSub;
    rclcpp::Subscription<base_interfaces::msg::GimbalPose>::SharedPtr gimbal_pitchAngle_yawSpeed_Sub;
    rclcpp::Publisher<std_msgs::msg::Char>::SharedPtr publishKey;
    
    void topAndSpeedCallback()
    {
        
        switch(c)
        {
            /**********底盘***********/
            case 'i':   // x方向运动
                chassis_x_speed += 0.1;
                break;
            case 'k':   // x反方向运动
                chassis_x_speed -= 0.1;
                break;
            case 'j':   // y方向运动
                chassis_y_speed += 0.1;
                break;
            case 'l':   // y反方向运动
                chassis_y_speed -= 0.1;
                break;
            case 't':
            case 'T':
                top = !top;
                break;
            /**********云台***********/
            case 'w':   // pitch方向运动
                gimbal_pitch_angle += 0.1;
                break;
            case 's':   // pitch反方向运动
                gimbal_pitch_angle -= 0.1;
                break;
            case 'a':   // yaw方向运动
                gimbal_yaw_angle += 0.1;
                break;
            case 'd':   // yaw反方向运动
                gimbal_yaw_angle -= 0.1;
                break;
            case 'e':
                use_angle_control = !use_angle_control;
                break;
            /**********射击***********/
            case '1':   // 单发
                bulletnum = 1;
                break;
            case '2':   // 5个连发
                bulletnum = 5;
                break;
            case '3':   // 持续发射
                bulletnum = -1;
                break;
            case '4':   // 停止发射
                bulletnum = 0;
                break;
            /*********发包权限管理***********/
            case 'C':
                chassis_enable = !chassis_enable;
                break;
            case 'G':
                gimbal_enbale = !gimbal_enbale;
                break;
            case 'S':
                shooter_enable = !shooter_enable;
                break;
            /***********控制权限切换*********/
            case 'D':
                permissions = 'D'; //AIMER
                break;
            case 'E':
                permissions = 'E'; //AIMER
                break;
            case 'N':
                permissions = 'N'; //NAVIGATION
                break;
            case 'A':
                permissions = 'A'; //SCAN
                break;
            /***********复位***********/
            case 'r':
                init();
                break;
	        default:
	            break;
        }
        c = 'f' ;
        usleep(1000);

        base_interfaces::msg::Chassis chassis_msg;
        chassis_msg.x_speed = chassis_x_speed;
        chassis_msg.y_speed = chassis_y_speed; 
        chassis_msg.power = top;
        if(chassis_enable)
        {
            topAndSpeed->publish(chassis_msg);
        }
        if(!chassis_enable && top)
        {
            chassis_msg.x_speed = 0;
            chassis_msg.y_speed = 0; 
            chassis_msg.power = top;
            topAndSpeed->publish(chassis_msg);
        }

        base_interfaces::msg::GimbalPose gimbal_msg;
        gimbal_msg.pitch = gimbal_pitch_angle;
        gimbal_msg.yaw = gimbal_yaw_angle;
        if(gimbal_enbale)
        {
            if(use_angle_control)
                setGimbalAngle->publish(gimbal_msg);
            else
                setGimbal_yawSpeed_pitchAngle->publish(gimbal_msg);
        }

        base_interfaces::msg::Shooter shooter_msg;
        shooter_msg.bulletnum = bulletnum;
        if(shooter_enable)
        {
            shooter->publish(shooter_msg);
        }
        if(bulletnum != -1)
            bulletnum = 0;

        std_msgs::msg::Char permission;
        if(permissions != 0)
        {
            permission.data = permissions;
            publishKey->publish(permission);
        }
    }

    void getGimbalPoseCallback(const base_interfaces::msg::GimbalPose::SharedPtr msg)
    {
        if(use_angle_control)
        {
            recv_gimbal_pitch_angle = msg->pitch;
            recv_gimbal_yaw_angle = msg->yaw;
        }
    }

    void getGimbalPoseCallback2(const base_interfaces::msg::GimbalPose::SharedPtr msg)
    {
        if(!use_angle_control)
        {
            recv_gimbal_pitch_angle = msg->pitch;
            recv_gimbal_yaw_angle = msg->yaw;
        }
    }

    void getChassisSpeedCallback(const base_interfaces::msg::Chassis::SharedPtr msg)
    {
        recv_chassis_x_speed = msg->x_speed;
        recv_chassis_y_speed = msg->y_speed;
        recv_top = msg->power;
    }

    void init()
    {
        // 底盘
        chassis_x_speed = 0.0;
        chassis_y_speed = 0.0;
        top = false;

        recv_chassis_x_speed = 0.0;
        recv_chassis_y_speed = 0.0;
        recv_top = false;
        // 云台
        gimbal_pitch_angle = 0.0;
        gimbal_yaw_angle = 0.0;
        
        recv_gimbal_pitch_angle = 0.0;
        recv_gimbal_yaw_angle = 0.0;

        use_angle_control = true;

        // 射击
        bulletnum = -2;

        chassis_enable = false;
        gimbal_enbale  = false;
        shooter_enable = false;

        permissions = 0;

        base_interfaces::msg::Chassis chassis_msg;
        chassis_msg.x_speed = chassis_x_speed;
        chassis_msg.y_speed = chassis_y_speed; 
        chassis_msg.power = top;
        topAndSpeed->publish(chassis_msg);

        base_interfaces::msg::GimbalPose gimbal_msg;
        gimbal_msg.pitch = gimbal_pitch_angle;
        gimbal_msg.yaw = gimbal_yaw_angle;
        setGimbalAngle->publish(gimbal_msg);

        base_interfaces::msg::Shooter shooter_msg;
        shooter_msg.bulletnum = bulletnum;
        shooter->publish(shooter_msg);
        
        std_msgs::msg::Char permission;
        permission.data = 'D';
        publishKey->publish(permission);
    }

    void printStatus()
    {
        if(chassis_enable || gimbal_enbale || shooter_enable || top)
            std::cout << "************************" << std::endl;
        if(chassis_enable)
            std::cout << "chassis enable"           << std::endl;
        if(gimbal_enbale)
            std::cout << "gimbal enbale"            << std::endl;
        if(shooter_enable)
            std::cout << "shooter enable"           << std::endl;
        if(top)
            std::cout << "top enable"               << std::endl;
        switch (permissions)
        {
        case 'D':
            std::cout << "gimbal control permission DEFAULT"    << std::endl;
            break;
        case 'E':
            std::cout << "gimbal control permission AIMER"      << std::endl;
            break;
        case 'N':
            std::cout << "gimbal control permission NAVIGATION" << std::endl;
            break;
        case 'A':
            std::cout << "gimbal control permission SCAN"       << std::endl;
            break;
        default:
            break;
        }
    }

    void print()
    {
        
        printStatus();
        std::cout << "************************"                 << std::endl
                  << "Gimbal: "                                 << std::endl
                  << " send: "                                  << std::endl
                  << "  pitch: "    << gimbal_pitch_angle       << std::endl
                  << "  yaw: "      << gimbal_yaw_angle         << std::endl
                  << " recv: "                                  << std::endl
                  << "  pitch: "    << recv_gimbal_pitch_angle  << std::endl
                  << "  yaw: "      << recv_gimbal_yaw_angle    << std::endl
                  << "Chassis: "                                << std::endl
                  << " send: "                                  << std::endl
                  << "  x_speed: "  << chassis_x_speed          << std::endl
                  << "  y_speed: "  << chassis_y_speed          << std::endl
                  << " recv: "                                  << std::endl    
                  << "  x_speed: "  << recv_chassis_x_speed     << std::endl
                  << "  y_speed: "  << recv_chassis_y_speed     << std::endl;
    }

};

int main(int argc, char **argv)
{
   rclcpp::init(argc, argv);
    /*产生一个test节点*/
    auto node = std::make_shared<ControlTest>("controltest");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;

}
